#include <ros/ros.h> 
#include <tf/tf.h>
#include <std_msgs/Int32MultiArray.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <math.h>


// 手臂部分marker半径
#define left_upper_radius 0.034         // 左侧大臂
#define right_upper_radius 0.034        // 右侧大臂
#define left_forearm_radius 0.028       // 左侧小臂
#define right_forearm_radius 0.028      // 右侧小臂
// 高度z_b + z_a * i
#define left_upper_z_b 0.085            // 左侧大臂
#define left_upper_z_a 0.018

#define right_upper_z_b 0.085           // 右侧大臂
#define right_upper_z_a 0.018

#define left_forearm_z_b 0.067          // 左侧小臂
#define left_forearm_z_a 0.018

#define right_forearm_z_b 0.067         // 右侧小臂
#define right_forearm_z_a 0.018

#define belly_z_b 0.09                  // 腹部
#define belly_z_a 0.015
#define belly_hor_dis 0.015         // 水平距离

#define left_shoulder_z_b 0.02          // 左侧肩膀
#define left_shoulder_z_a 0.015
#define left_shoulder_hor_dis 0.015 // 水平距离

#define right_shoulder_z_b 0.02          // 右侧肩膀
#define right_shoulder_z_a 0.015
#define right_shoulder_hor_dis 0.015 // 水平距离

std_msgs::Int32MultiArray read_data_0;
std_msgs::Int32MultiArray read_data_1;
std_msgs::Int32MultiArray read_data_2;
std_msgs::Int32MultiArray read_data_3;
std_msgs::Int32MultiArray read_data_4;

void get_force_data_0(const std_msgs::Int32MultiArray::ConstPtr& newdata)
{
    ROS_INFO("Force Data 0 Received!");
    //read_data.data.clear();
    read_data_0.data = newdata->data;
    
}

void get_force_data_1(const std_msgs::Int32MultiArray::ConstPtr& newdata)
{
    ROS_INFO("Force Data 1 Received!");
    //read_data.data.clear();
    read_data_1.data = newdata->data;
    
}

void get_force_data_2(const std_msgs::Int32MultiArray::ConstPtr& newdata)
{
    ROS_INFO("Force Data 2 Received!");
    //read_data.data.clear();
    read_data_2.data = newdata->data;
    
}

void get_force_data_3(const std_msgs::Int32MultiArray::ConstPtr& newdata)
{
    ROS_INFO("Force Data 3 Received!");
    //read_data.data.clear();
    read_data_3.data = newdata->data;
    
}

void get_force_data_4(const std_msgs::Int32MultiArray::ConstPtr& newdata)
{
    ROS_INFO("Force Data 4 Received!");
    //read_data.data.clear();
    read_data_4.data = newdata->data;
    
}

// 根据压力大小得到颜色RGB
void get_color(int force, int* RGB, int* color_mode_ptr)       
{   
    int force_data = force;
    if(force_data > 3000){
        force_data = 3000;
    }
    /*********逐渐变色*************/
    // *color_mode_ptr = 0;
    //if (force < 500){
    //    *color_mode_ptr = 0;         // 不显示或显示灰色
    //}
    //else{
        /*
        *color_mode_ptr = 1;
        RGB[0] = (force_data/3000.0)*255;       
        RGB[1] = 0;
        RGB[2] = (3000 - force_data)/3000.0*255;
        */
    //}
    /*********分段变色(三段)*************/
    /*       
    *color_mode_ptr = 0;
    if (force < 500){
        *color_mode_ptr = 0;         // 不显示或显示灰色
    }
    else if (force >=500 && force < 1000){
        *color_mode_ptr = 1;         // 浅蓝色
    }
    else if (force >=1000 && force < 2000){
        *color_mode_ptr = 2;         // 暗红色
    }
    else {
        *color_mode_ptr = 3;         // 亮红色
    }
    ROS_INFO_STREAM("color mode is" << *color_mode_ptr);
    switch(*color_mode_ptr){
        case 0:
            RGB[0] = 105;       // 灰色
            RGB[1] = 105;
            RGB[2] = 105;
            break;
        case 1:
            RGB[0] = 0;       // 浅蓝色G206,B209
            RGB[1] = 0;
            RGB[2] = 255;
            break;
        case 2:
            RGB[0] = 0;       // 暗红色139,26,26
            RGB[1] = 255;
            RGB[2] = 0;
            break;
        case 3:
            RGB[0] = 255;       // 亮红色R238
            RGB[1] = 0;
            RGB[2] = 0;
            break;
    }
    */

   /*********分段变色(ansys九段)*************/    
    *color_mode_ptr = 0;
    if (force < 300){
        *color_mode_ptr = 0;         // 不显示或显示灰色
    }
    else if (force >=300 && force < 600){
        *color_mode_ptr = 1;         // 浅蓝色
    }
    else if (force >=600 && force < 900){
        *color_mode_ptr = 2;         // 暗红色
    }
    else if (force >=900 && force < 1200){
        *color_mode_ptr = 3;         // 暗红色
    }
    else if (force >=1200 && force < 1500){
        *color_mode_ptr = 4;         // 暗红色
    }
    else if (force >=1500 && force < 1800){
        *color_mode_ptr = 5;         // 暗红色
    }
    else if (force >=1800 && force < 2100){
        *color_mode_ptr = 6;         // 暗红色
    }
    else if (force >=2100 && force < 2400){
        *color_mode_ptr = 7;         // 暗红色
    }
    else if (force >=2400 && force < 2700){
        *color_mode_ptr = 8;         // 暗红色
    }
    else if (force >=2700 && force < 3000){
        *color_mode_ptr = 9;         // 暗红色
    }
    //ROS_INFO_STREAM("color mode is" << *color_mode_ptr);
    switch(*color_mode_ptr){
        case 0:
            RGB[0] = 105;       // 灰色
            RGB[1] = 105;
            RGB[2] = 105;
            break;
        case 1:
            RGB[0] = 0;       // 浅蓝色G206,B209
            RGB[1] = 0;
            RGB[2] = 255;
            break;
        case 2:
            RGB[0] = 30;       // 暗红色139,26,26
            RGB[1] = 144;
            RGB[2] = 255;
            break;
        case 3:
            RGB[0] = 0;       
            RGB[1] = 255;
            RGB[2] = 255;
            break;
        case 4:
            RGB[0] = 84;       
            RGB[1] = 255;
            RGB[2] = 159;
            break;
        case 5:
            RGB[0] = 173;       
            RGB[1] = 255;
            RGB[2] = 47;
            break;
        case 6:
            RGB[0] = 0;       
            RGB[1] = 255;
            RGB[2] = 0;
            break;
        case 7:
            RGB[0] = 255;       
            RGB[1] = 255;
            RGB[2] = 0;
            break;
        case 8:
            RGB[0] = 255;       
            RGB[1] = 165;
            RGB[2] = 0;
            break;
        case 9:
            RGB[0] = 238;       
            RGB[1] = 0;
            RGB[2] = 0;
            break;
    }
}

int sensor_classify_upper_arm(int num,int sensor_pos[][9])      // 根据marker的代号，找到对应的传感器(但并不是传感器对应的数据标号)
{
    int i,j;
    int result = -1;
    for(i = 0; i < 10; i++)
    {
        for(j = 0; j < sizeof(sensor_pos[0])/sizeof(sensor_pos[0][0]); j++)
        {
            if(sensor_pos[i][j] == num){
                result = i;
                return result;
            }
        }
    }
    return result;
}

int sensor_classify_forearm(int num,int sensor_pos[][9])      // 根据marker的代号，找到对应的传感器(但并不是传感器对应的数据标号)
{
    int i,j;
    int result = -1;
    for(i = 0; i < 8; i++)
    {
        for(j = 0; j < sizeof(sensor_pos[0])/sizeof(sensor_pos[0][0]); j++)
        {
            if(sensor_pos[i][j] == num){
                result = i;
                return result;
            }
        }
    }
    return result;
}

int left_upper_sensor_pos_convert(int sensor)              // 将传感器的标号转为数据的标号
{
    switch(sensor)
    {
        case 8:return 0;break;
        case 1:return 1;break;
        case 2:return 2;break;
        case 4:return 3;break;
        case 6:return 4;break;
        case 0:return 5;break;
        case 9:return 6;break;
        case 3:return 7;break;
        case 5:return 8;break;
        case 7:return 9;break;
    }
}

int right_upper_sensor_pos_convert(int sensor)              // 将传感器的标号转为数据的标号
{
    switch(sensor)
    {
        case 3:return 0;break;
        case 8:return 1;break;
        case 6:return 2;break;
        case 0:return 3;break;
        case 5:return 4;break;
        case 1:return 5;break;
        case 2:return 6;break;
        case 4:return 7;break;
        case 9:return 8;break;
        case 7:return 9;break;
    }
}

int left_forearm_sensor_pos_convert(int sensor)              // 将传感器的标号转为数据的标号
{
    switch(sensor)
    {
        case 0:return 0;break;
        case 6:return 1;break;
        case 4:return 2;break;
        case 2:return 3;break;
        case 7:return 4;break;
        case 5:return 5;break;
        case 3:return 6;break;
        case 1:return 7;break;
    }
}

int right_forearm_sensor_pos_convert(int sensor)              // 将传感器的标号转为数据的标号
{
    switch(sensor)
    {
        case 2:return 0;break;
        case 0:return 1;break;
        case 1:return 2;break;
        case 3:return 3;break;
        case 4:return 4;break;
        case 6:return 5;break;
        case 7:return 6;break;
        case 5:return 7;break;
    }
}





int main(int argc, char **argv)
{
    ros::init(argc, argv, "tactile_marker0");
    ros::NodeHandle n;
    ros::Rate loop_rate(200); 
    ros::Subscriber sub_0 = n.subscribe("force_data_topic0", 1000, get_force_data_0);
    ros::Subscriber sub_1 = n.subscribe("force_data_topic1", 1000, get_force_data_1);
    ros::Subscriber sub_2 = n.subscribe("force_data_topic2", 1000, get_force_data_2);
    ros::Subscriber sub_3 = n.subscribe("force_data_topic3", 1000, get_force_data_3);
    ros::Subscriber sub_4 = n.subscribe("force_data_topic4", 1000, get_force_data_4);
    ros::Publisher marker_pub = n.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 1);
    uint32_t shape = visualization_msgs::Marker::CYLINDER;

    int color_RBG[3];
    int color = 0;
    int *color_mode = &color;
    //int* p_RGB = color_RBG;

    int color_pos;                  // sensor_classify的输出     
    int *color_pos_ptr = &color_pos;

    float color_m;

    // marker与传感器对应关系
    //左大臂
    int sensor_pos_left_upper_arm[10][9] = {
        {36,37,38,42,43,44,18,19,20},        //0
        {21,22,23,51,52,53,45,46,47},
        {57,58,59,9,10,11,-1,-1,-1},
        {6,7,8,60,61,62,-1,-1,-1},
        {63,64,65,69,70,71,-1,-1,-1},
        {66,67,68,12,13,14,-1,-1,-1},        //5
        {15,16,17,27,28,29,-1,-1,-1},
        {30,31,32,24,25,26,0,1,2},
        {3,4,5,33,34,35,39,40,41},
        {48,49,50,54,55,56,-1,-1,-1}
    };
    int sensor_pos_right_upper_arm[10][9] = {
        {21,22,23,51,52,53,57,58,59},
        {24,25,26,12,13,14,-1,-1,-1},
        {6,7,8,60,61,62,66,67,68},
        {33,34,35,27,28,29,-1,-1,-1},
        {48,49,50,54,55,56,-1,-1,-1},
        {3,4,5,39,40,41,45,46,47},
        {63,64,65,9,10,11,-1,-1,-1},
        {0,1,2,30,31,32,36,37,38},
        {15,16,17,69,70,71,-1,-1,-1},
        {18,19,20,42,43,44,-1,-1,-1},
    };
    int sensor_pos_left_forearm[8][9] = {
        {3,4,5,33,34,35,39,40,41},
        {18,19,20,42,43,44,36,37,38},
        {51,52,53,21,22,23,45,46,47},
        {6,7,8,54,55,56,48,49,50},
        {63,64,65,9,10,11,57,58,59},
        {12,13,14,66,67,68,60,61,62},
        {27,28,29,15,16,17,69,70,71},
        {0,1,2,30,31,32,24,25,26}
    };
    int sensor_pos_right_forearm[8][9] = {
        {24,25,26,12,13,14,66,67,68},
        {6,7,8,60,61,62,54,55,56},
        {0,1,2,30,31,32,36,37,38},
        {48,49,50,18,19,20,42,43,44},
        {33,34,35,27,28,29,15,16,17},
        {3,4,5,39,40,41,45,46,47},
        {69,70,71,63,64,65,9,10,11},
        {57,58,59,51,52,53,21,22,23} 
    };

    while (ros::ok())
    {

    visualization_msgs::MarkerArray marker_array;
    marker_array.markers.clear();
    
    visualization_msgs::Marker bbox_marker;

    geometry_msgs::Quaternion q;
    double roll,pitch,yaw;

    //marker_pub.publish(marker_array);

    for(int k = 0; k < 10; k++)
    {
        read_data_0.data.push_back(0.0);
        read_data_1.data.push_back(0.0);
        read_data_2.data.push_back(0.0);
        read_data_3.data.push_back(0.0);
        read_data_4.data.push_back(0.0);
    }
    
    // 左侧大臂
    for(int i = 0; i < 72 ;i++)
    {
        
        bbox_marker.header.frame_id = "/left_upper_arm_link";
        bbox_marker.header.stamp = ros::Time::now();
        bbox_marker.ns = "basic_shapes";
        bbox_marker.id = i;
        bbox_marker.type = shape;
        bbox_marker.action = visualization_msgs::Marker::ADD;


        //bbox_marker.scale.x = 0.01;
        //bbox_marker.scale.y = 0.01;
        //bbox_marker.scale.z = 0.0001;

        //bbox_marker.color.a = 0.8;
        
        if(i < 6)
        {
            bbox_marker.pose.position.x = 0.0;
            bbox_marker.pose.position.y = left_upper_radius;            //0.034
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * i;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
            /*
            get_color(read_data_0.data[0], color_RBG, color_mode);
            bbox_marker.color.r = color_RBG[0]/255.0;
            bbox_marker.color.g = color_RBG[1]/255.0;
            bbox_marker.color.b = color_RBG[2]/255.0;
            */
        }
        if(i >= 6 && i < 12)
        {
            bbox_marker.pose.position.x = 0.0;
            bbox_marker.pose.position.y = -left_upper_radius;            
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 6);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 12 && i < 18)
        {
            bbox_marker.pose.position.x = left_upper_radius;
            bbox_marker.pose.position.y = 0.0;            
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 12);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 18 && i < 24)
        {
            bbox_marker.pose.position.x = -left_upper_radius;
            bbox_marker.pose.position.y = 0.0;            
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 18);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 24 && i < 30)           // 绕Z轴旋转30度
        {
            bbox_marker.pose.position.x = left_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = left_upper_radius * sin(30.0 / 180.0 * 3.1415926);            
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 24);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.5235987756;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 30 && i < 36)           // 绕Z轴旋转60度
        {
            bbox_marker.pose.position.x = left_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = left_upper_radius * cos(30.0 / 180.0 * 3.1415926);           
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 30);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 1.047197551;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 36 && i < 42)               // 绕Z轴旋转120度
        {
            bbox_marker.pose.position.x = -left_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = left_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 36);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 2.094395102;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 42 && i < 48)           // 绕Z轴旋转150度
        {
            bbox_marker.pose.position.x = -left_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = left_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 42);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 2.617993878;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 48 && i < 54)           // 绕Z轴旋转210度
        {
            bbox_marker.pose.position.x = -left_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -left_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 48);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 3.665191429;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 54 && i < 60)           // 绕Z轴旋转240度
        {
            bbox_marker.pose.position.x = -left_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -left_upper_radius * cos(30.0 / 180.0 * 3.1415926);            
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 54);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 4.188790205;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 60 && i < 66)           // 绕Z轴旋转300度
        {
            bbox_marker.pose.position.x = left_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -left_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 60);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = -1.047197551;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 66 && i < 72)           // 绕Z轴旋转330度
        {
            bbox_marker.pose.position.x = left_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -left_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_upper_z_b + left_upper_z_a * (i - 66);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = -0.5235987756;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }

        color_pos = left_upper_sensor_pos_convert(sensor_classify_upper_arm(i, sensor_pos_left_upper_arm));
        //ROS_INFO_STREAM("color_pos is" << *color_pos_ptr);
        //ROS_INFO_STREAM("read_data is" << read_data_0.data[color_pos]);
        
        get_color(read_data_1.data[color_pos], color_RBG, color_mode);

        bbox_marker.color.r = color_RBG[0]/255.0;
        bbox_marker.color.g = color_RBG[1]/255.0;
        bbox_marker.color.b = color_RBG[2]/255.0;

        if(*color_mode == 0){
            bbox_marker.color.a = 0.0;
            bbox_marker.scale.x = 0.0;
            bbox_marker.scale.y = 0.0;
            bbox_marker.scale.z = 0.0;
        }
        else{
            bbox_marker.color.a = 0.8;
            bbox_marker.scale.x = 0.01;
            bbox_marker.scale.y = 0.01;
            bbox_marker.scale.z = 0.0001;
        }
        
        bbox_marker.lifetime = ros::Duration();
        marker_array.markers.push_back(bbox_marker);
        
    }
    
    // 右侧大臂
    for(int i = 0; i < 72 ;i++)
    {
        
        bbox_marker.header.frame_id = "/right_upper_arm_link";
        bbox_marker.header.stamp = ros::Time::now();
        bbox_marker.ns = "basic_shapes";
        bbox_marker.id = i + 72;
        bbox_marker.type = shape;
        bbox_marker.action = visualization_msgs::Marker::ADD;


        //bbox_marker.scale.x = 0.01;
        //bbox_marker.scale.y = 0.01;
        //bbox_marker.scale.z = 0.0001;

        //bbox_marker.color.a = 0.8;
        
        if(i < 6)
        {
            bbox_marker.pose.position.x = 0.0;
            bbox_marker.pose.position.y = right_upper_radius;            //0.034
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * i;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 6 && i < 12)
        {
            bbox_marker.pose.position.x = 0.0;
            bbox_marker.pose.position.y = -right_upper_radius;            
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 6);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 12 && i < 18)
        {
            bbox_marker.pose.position.x = right_upper_radius;
            bbox_marker.pose.position.y = 0.0;            
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 12);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 18 && i < 24)
        {
            bbox_marker.pose.position.x = -right_upper_radius;
            bbox_marker.pose.position.y = 0.0;            
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 18);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 24 && i < 30)           // 绕Z轴旋转30度
        {
            bbox_marker.pose.position.x = right_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = right_upper_radius * sin(30.0 / 180.0 * 3.1415926);            
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 24);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.5235987756;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 30 && i < 36)           // 绕Z轴旋转60度
        {
            bbox_marker.pose.position.x = right_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = right_upper_radius * cos(30.0 / 180.0 * 3.1415926);           
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 30);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 1.047197551;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 36 && i < 42)               // 绕Z轴旋转120度
        {
            bbox_marker.pose.position.x = -right_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = right_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 36);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 2.094395102;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 42 && i < 48)           // 绕Z轴旋转150度
        {
            bbox_marker.pose.position.x = -right_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = right_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 42);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 2.617993878;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 48 && i < 54)           // 绕Z轴旋转210度
        {
            bbox_marker.pose.position.x = -right_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -right_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 48);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 3.665191429;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 54 && i < 60)           // 绕Z轴旋转240度
        {
            bbox_marker.pose.position.x = -right_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -right_upper_radius * cos(30.0 / 180.0 * 3.1415926);            
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 54);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 4.188790205;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 60 && i < 66)           // 绕Z轴旋转300度
        {
            bbox_marker.pose.position.x = right_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -right_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 60);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = -1.047197551;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 66 && i < 72)           // 绕Z轴旋转330度
        {
            bbox_marker.pose.position.x = right_upper_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -right_upper_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_upper_z_b + right_upper_z_a * (i - 66);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = -0.5235987756;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        
        color_pos = right_upper_sensor_pos_convert(sensor_classify_upper_arm(i, sensor_pos_right_upper_arm));
        //ROS_INFO_STREAM("color_pos is" << *color_pos_ptr);
        //ROS_INFO_STREAM("read_data is" << read_data_1.data[color_pos]);
        
        get_color(read_data_0.data[color_pos], color_RBG, color_mode);

        bbox_marker.color.r = color_RBG[0]/255.0;
        bbox_marker.color.g = color_RBG[1]/255.0;
        bbox_marker.color.b = color_RBG[2]/255.0;
        
        
        if(*color_mode == 0){
            bbox_marker.color.a = 0.0;
            bbox_marker.scale.x = 0.0;
            bbox_marker.scale.y = 0.0;
            bbox_marker.scale.z = 0.0;
        }
        else{
            bbox_marker.color.a = 0.8;
            bbox_marker.scale.x = 0.01;
            bbox_marker.scale.y = 0.01;
            bbox_marker.scale.z = 0.0001;
        }
        
       /*
        if(i == 40){
            bbox_marker.color.a = 0.8;
            bbox_marker.scale.x = 0.01;
            bbox_marker.scale.y = 0.01;
            bbox_marker.scale.z = 0.0001;
            bbox_marker.color.r = 255.0;
            bbox_marker.color.g = 0.0;
            bbox_marker.color.b = 0.0;
        }
        else{
            bbox_marker.color.a = 0.0;
            bbox_marker.scale.x = 0.0;
            bbox_marker.scale.y = 0.0;
            bbox_marker.scale.z = 0.0;
        }
        */
        bbox_marker.lifetime = ros::Duration();
        marker_array.markers.push_back(bbox_marker);
    }

    // 左侧小臂
    for(int i = 0; i < 72 ;i++)
    {
        
        bbox_marker.header.frame_id = "/left_forearm_link";
        bbox_marker.header.stamp = ros::Time::now();
        bbox_marker.ns = "basic_shapes";
        bbox_marker.id = i + 72 + 72;
        bbox_marker.type = shape;
        bbox_marker.action = visualization_msgs::Marker::ADD;


        //bbox_marker.scale.x = 0.01;
        //bbox_marker.scale.y = 0.01;
        //bbox_marker.scale.z = 0.0001;
        
        //bbox_marker.color.a = 0.8;

        if(i < 6)
        {
            bbox_marker.pose.position.x = 0.0;
            bbox_marker.pose.position.y = left_forearm_radius;            //0.034
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * i;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 6 && i < 12)
        {
            bbox_marker.pose.position.x = 0.0;
            bbox_marker.pose.position.y = -left_forearm_radius;            
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 6);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 12 && i < 18)
        {
            bbox_marker.pose.position.x = left_forearm_radius;
            bbox_marker.pose.position.y = 0.0;            
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 12);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 18 && i < 24)
        {
            bbox_marker.pose.position.x = -left_forearm_radius;
            bbox_marker.pose.position.y = 0.0;            
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 18);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 24 && i < 30)           // 绕Z轴旋转30度
        {
            bbox_marker.pose.position.x = left_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = left_forearm_radius * sin(30.0 / 180.0 * 3.1415926);            
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 24);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.5235987756;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 30 && i < 36)           // 绕Z轴旋转60度
        {
            bbox_marker.pose.position.x = left_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = left_forearm_radius * cos(30.0 / 180.0 * 3.1415926);           
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 30);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 1.047197551;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 36 && i < 42)               // 绕Z轴旋转120度
        {
            bbox_marker.pose.position.x = -left_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = left_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 36);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 2.094395102;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 42 && i < 48)           // 绕Z轴旋转150度
        {
            bbox_marker.pose.position.x = -left_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = left_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 42);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 2.617993878;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 48 && i < 54)           // 绕Z轴旋转210度
        {
            bbox_marker.pose.position.x = -left_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -left_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 48);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 3.665191429;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 54 && i < 60)           // 绕Z轴旋转240度
        {
            bbox_marker.pose.position.x = -left_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -left_forearm_radius * cos(30.0 / 180.0 * 3.1415926);            
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 54);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 4.188790205;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 60 && i < 66)           // 绕Z轴旋转300度
        {
            bbox_marker.pose.position.x = left_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -left_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 60);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = -1.047197551;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 66 && i < 72)           // 绕Z轴旋转330度
        {
            bbox_marker.pose.position.x = left_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -left_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = left_forearm_z_b + left_forearm_z_a * (i - 66);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = -0.5235987756;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        color_pos = left_forearm_sensor_pos_convert(sensor_classify_forearm(i, sensor_pos_left_forearm));
        //ROS_INFO_STREAM("color_pos is" << *color_pos_ptr);
        //ROS_INFO_STREAM("read_data is" << read_data_2.data[color_pos]);
        
        get_color(read_data_2.data[color_pos], color_RBG, color_mode);

        bbox_marker.color.r = color_RBG[0]/255.0;
        bbox_marker.color.g = color_RBG[1]/255.0;
        bbox_marker.color.b = color_RBG[2]/255.0;

        if(*color_mode == 0){
            bbox_marker.color.a = 0.0;
            bbox_marker.scale.x = 0.0;
            bbox_marker.scale.y = 0.0;
            bbox_marker.scale.z = 0.0;
        }
        else{
            bbox_marker.color.a = 0.8;
            bbox_marker.scale.x = 0.01;
            bbox_marker.scale.y = 0.01;
            bbox_marker.scale.z = 0.0001;
        }
        
        bbox_marker.lifetime = ros::Duration();
        marker_array.markers.push_back(bbox_marker);
    }
  
    // 右侧小臂
    for(int i = 0; i < 72 ;i++)
    {
        
        bbox_marker.header.frame_id = "/right_forearm_link";
        bbox_marker.header.stamp = ros::Time::now();
        bbox_marker.ns = "basic_shapes";
        bbox_marker.id = i + 72 + 72 + 72;
        bbox_marker.type = shape;
        bbox_marker.action = visualization_msgs::Marker::ADD;


        //bbox_marker.scale.x = 0.01;
        //bbox_marker.scale.y = 0.01;
        //bbox_marker.scale.z = 0.0001;
        
        //bbox_marker.color.a = 0.8;

        if(i < 6)
        {
            bbox_marker.pose.position.x = 0.0;
            bbox_marker.pose.position.y = right_forearm_radius;            //0.034
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * i;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 6 && i < 12)
        {
            bbox_marker.pose.position.x = 0.0;
            bbox_marker.pose.position.y = -right_forearm_radius;            
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 6);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 12 && i < 18)
        {
            bbox_marker.pose.position.x = right_forearm_radius;
            bbox_marker.pose.position.y = 0.0;            
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 12);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 18 && i < 24)
        {
            bbox_marker.pose.position.x = -right_forearm_radius;
            bbox_marker.pose.position.y = 0.0;            
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 18);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 24 && i < 30)           // 绕Z轴旋转30度
        {
            bbox_marker.pose.position.x = right_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = right_forearm_radius * sin(30.0 / 180.0 * 3.1415926);            
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 24);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 0.5235987756;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 30 && i < 36)           // 绕Z轴旋转60度
        {
            bbox_marker.pose.position.x = right_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = right_forearm_radius * cos(30.0 / 180.0 * 3.1415926);           
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 30);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 1.047197551;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 36 && i < 42)               // 绕Z轴旋转120度
        {
            bbox_marker.pose.position.x = -right_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = right_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 36);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 2.094395102;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 42 && i < 48)           // 绕Z轴旋转150度
        {
            bbox_marker.pose.position.x = -right_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = right_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 42);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 2.617993878;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 48 && i < 54)           // 绕Z轴旋转210度
        {
            bbox_marker.pose.position.x = -right_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -right_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 48);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 3.665191429;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 54 && i < 60)           // 绕Z轴旋转240度
        {
            bbox_marker.pose.position.x = -right_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -right_forearm_radius * cos(30.0 / 180.0 * 3.1415926);            
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 54);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = 4.188790205;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 60 && i < 66)           // 绕Z轴旋转300度
        {
            bbox_marker.pose.position.x = right_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -right_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 60);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = -1.047197551;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 66 && i < 72)           // 绕Z轴旋转330度
        {
            bbox_marker.pose.position.x = right_forearm_radius * cos(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.y = -right_forearm_radius * sin(30.0 / 180.0 * 3.1415926);
            bbox_marker.pose.position.z = right_forearm_z_b + right_forearm_z_a * (i - 66);
            roll = 0.0;
            pitch = 1.570796327;
            yaw = -0.5235987756;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        color_pos = right_forearm_sensor_pos_convert(sensor_classify_forearm(i, sensor_pos_right_forearm));
        //ROS_INFO_STREAM("color_pos is" << *color_pos_ptr);
        //ROS_INFO_STREAM("read_data is" << read_data_3.data[color_pos]);
        
        get_color(read_data_3.data[color_pos], color_RBG, color_mode);

        bbox_marker.color.r = color_RBG[0]/255.0;
        bbox_marker.color.g = color_RBG[1]/255.0;
        bbox_marker.color.b = color_RBG[2]/255.0;

        if(*color_mode == 0){
            bbox_marker.color.a = 0.0;
            bbox_marker.scale.x = 0.0;
            bbox_marker.scale.y = 0.0;
            bbox_marker.scale.z = 0.0;
        }
        else{
            bbox_marker.color.a = 0.8;
            bbox_marker.scale.x = 0.01;
            bbox_marker.scale.y = 0.01;
            bbox_marker.scale.z = 0.0001;
        }
        
        bbox_marker.lifetime = ros::Duration();
        marker_array.markers.push_back(bbox_marker);
    }

    // 下腹肌
    for(int i = 0; i < 64 ;i++)
    {
        
        bbox_marker.header.frame_id = "/base_link";
        bbox_marker.header.stamp = ros::Time::now();
        bbox_marker.ns = "basic_shapes";
        bbox_marker.id = i + 72 + 72 + 72 + 72;
        bbox_marker.type = shape;
        bbox_marker.action = visualization_msgs::Marker::ADD;


        bbox_marker.scale.x = 0.01;
        bbox_marker.scale.y = 0.01;
        bbox_marker.scale.z = 0.0001;


        if(read_data_4.data[i] == 1)
        {

            bbox_marker.color.a = 0.8;
            bbox_marker.color.r = 0.8;
            bbox_marker.color.g = 0.0;
            bbox_marker.color.b = 0.0;
        }
        else
        {
            bbox_marker.color.a = 0.8;
            bbox_marker.color.r = 0.8;
            bbox_marker.color.g = 0.8;
            bbox_marker.color.b = 0.8;
        }
            
        
        if(i < 8)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * i;
            bbox_marker.pose.position.y = -0.087;            //0.034
            bbox_marker.pose.position.z = 0.28 - 0.015 * 7;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 8 && i < 16)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * (i - 8);
            bbox_marker.pose.position.y = -0.087;            //0.034
            bbox_marker.pose.position.z = 0.28 - 0.015 * 6;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 16 && i < 24)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * (i - 16);
            bbox_marker.pose.position.y = -0.087;            //0.034
            bbox_marker.pose.position.z = 0.28 - 0.015 * 5;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 24 && i < 32)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * (i - 24);
            bbox_marker.pose.position.y = -0.087;            //0.034
            bbox_marker.pose.position.z = 0.28 - 0.015 * 4;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 32 && i < 40)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * (i - 32);
            bbox_marker.pose.position.y = -0.087;            //0.034
            bbox_marker.pose.position.z = 0.28 - 0.015 * 3;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 40 && i < 48)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * (i - 40);
            bbox_marker.pose.position.y = -0.087;            //0.034
            bbox_marker.pose.position.z = 0.28 - 0.015 * 2;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }

        if(i >= 48 && i < 56)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * (i - 48);
            bbox_marker.pose.position.y = -0.087;            //0.034
            bbox_marker.pose.position.z = 0.28 - 0.015 * 1;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 56 && i < 64)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * (i - 56);
            bbox_marker.pose.position.y = -0.087;            //0.034
            bbox_marker.pose.position.z = 0.28;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }

        

        bbox_marker.lifetime = ros::Duration();
        marker_array.markers.push_back(bbox_marker);
    }

    // 上腹肌
    for(int i = 0; i < 64 ;i++)
    {
        
        bbox_marker.header.frame_id = "/base_link";
        bbox_marker.header.stamp = ros::Time::now();
        bbox_marker.ns = "basic_shapes";
        bbox_marker.id = i + 72 + 72 + 72 + 72 + 64;
        bbox_marker.type = shape;
        bbox_marker.action = visualization_msgs::Marker::ADD;


        bbox_marker.scale.x = 0.01;
        bbox_marker.scale.y = 0.01;
        bbox_marker.scale.z = 0.0001;

        

        if(read_data_4.data[i + 64] == 1)
        {

            bbox_marker.color.a = 0.8;
            bbox_marker.color.r = 0.8;
            bbox_marker.color.g = 0.0;
            bbox_marker.color.b = 0.0;
        }
        else
        {
            bbox_marker.color.a = 0.8;
            bbox_marker.color.r = 0.8;
            bbox_marker.color.g = 0.8;
            bbox_marker.color.b = 0.8;
        }
        
        if(i < 8)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * i;
            bbox_marker.pose.position.y = -0.087;            //0.034
            bbox_marker.pose.position.z = 0.28 + 0.015 * 1;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 8 && i < 16)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * (i - 8);
            bbox_marker.pose.position.y = -0.087;            //0.034
            bbox_marker.pose.position.z = 0.28 + 0.015 * 2;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 16 && i < 24)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * (i - 16);
            bbox_marker.pose.position.y = -0.087;            //0.034
            bbox_marker.pose.position.z = 0.28 + 0.015 * 3;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 24 && i < 32)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * (i - 24);
            bbox_marker.pose.position.y = -0.087;            //0.034
            bbox_marker.pose.position.z = 0.28 + 0.015 * 4;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 32 && i < 40)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * (i - 32);
            bbox_marker.pose.position.y = -0.087;            //0.034
            bbox_marker.pose.position.z = 0.28 + 0.015 * 5;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 40 && i < 48)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * (i - 40);
            bbox_marker.pose.position.y = -0.087;            //0.034
            bbox_marker.pose.position.z = 0.28 + 0.015 * 6;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }

        if(i >= 48 && i < 56)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * (i - 48);
            bbox_marker.pose.position.y = -0.087;            //0.034
            bbox_marker.pose.position.z = 0.28 + 0.015 * 7;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 56 && i < 64)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * (i - 56);
            bbox_marker.pose.position.y = -0.087;            //0.034
            bbox_marker.pose.position.z = 0.28 + 0.015 * 8;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }

     

        bbox_marker.lifetime = ros::Duration();
        marker_array.markers.push_back(bbox_marker);
    }
      

    // 左肩
    for (int i = 0; i < 64; i++)
    {
        bbox_marker.header.frame_id = "/left_base";
        bbox_marker.header.stamp = ros::Time::now();
        bbox_marker.ns = "basic_shapes";
        bbox_marker.id = i + 72 + 72 + 72 + 72 + 128;
        bbox_marker.type = shape;
        bbox_marker.action = visualization_msgs::Marker::ADD;


        if(read_data_4.data[i + 64 + 64] == 1 && i%8 != 0)
        {
            
            bbox_marker.color.a = 0.8;
            bbox_marker.color.r = 0.8;
            bbox_marker.color.g = 0.0;
            bbox_marker.color.b = 0.0;
        }
        else
        {
            bbox_marker.color.a = 0.8;
            bbox_marker.color.r = 0.8;
            bbox_marker.color.g = 0.8;
            bbox_marker.color.b = 0.8;
        }

        /*
        bbox_marker.scale.x = 0.0;
        bbox_marker.scale.y = 0.0;
        bbox_marker.scale.z = 0.0;

        bbox_marker.color.a = 0.0;
        */

        if(i < 8)
        {
            bbox_marker.pose.position.x = -0.055;
            bbox_marker.pose.position.y = 0.09;            
            bbox_marker.pose.position.z = -left_shoulder_z_b - left_shoulder_z_a * i;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        
        if(i >= 8 && i < 16)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015;
            bbox_marker.pose.position.y = 0.09;            
            bbox_marker.pose.position.z = -left_shoulder_z_b - left_shoulder_z_a * (i - 8);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 16 && i < 24)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * 2;
            bbox_marker.pose.position.y = 0.09;            
            bbox_marker.pose.position.z = -left_shoulder_z_b - left_shoulder_z_a * (i - 16);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 24 && i < 32)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * 3;
            bbox_marker.pose.position.y = 0.09;            
            bbox_marker.pose.position.z = -left_shoulder_z_b - left_shoulder_z_a * (i - 24);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 32 && i < 40)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * 4;
            bbox_marker.pose.position.y = 0.09;            
            bbox_marker.pose.position.z = -left_shoulder_z_b - left_shoulder_z_a * (i - 32);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 40 && i < 48)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * 5;
            bbox_marker.pose.position.y = 0.09;            
            bbox_marker.pose.position.z = -left_shoulder_z_b - left_shoulder_z_a * (i - 40);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 48 && i < 56)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * 6;
            bbox_marker.pose.position.y = 0.09;            
            bbox_marker.pose.position.z = -left_shoulder_z_b - left_shoulder_z_a * (i - 48);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 56 && i < 64)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * 7;
            bbox_marker.pose.position.y = 0.09;            
            bbox_marker.pose.position.z = -left_shoulder_z_b - left_shoulder_z_a * (i - 56);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        
        bbox_marker.lifetime = ros::Duration();
        marker_array.markers.push_back(bbox_marker);
    }

    // 右肩
    for (int i = 0; i < 64; i++)
    {
        bbox_marker.header.frame_id = "/right_base";
        bbox_marker.header.stamp = ros::Time::now();
        bbox_marker.ns = "basic_shapes";
        bbox_marker.id = i + 72 + 72 + 72 + 72 + 128 + 64;
        bbox_marker.type = shape;
        bbox_marker.action = visualization_msgs::Marker::ADD;


        if(read_data_4.data[i + 64 + 64 + 64] == 1)
        {

            bbox_marker.color.a = 0.8;
            bbox_marker.color.r = 0.8;
            bbox_marker.color.g = 0.0;
            bbox_marker.color.b = 0.0;
        }
        else
        {
            bbox_marker.color.a = 0.8;
            bbox_marker.color.r = 0.8;
            bbox_marker.color.g = 0.8;
            bbox_marker.color.b = 0.8;
        }
        /*
        bbox_marker.scale.x = 0.0;
        bbox_marker.scale.y = 0.0;
        bbox_marker.scale.z = 0.0;

        bbox_marker.color.a = 0.0;
        */      

        if(i < 8)
        {
            bbox_marker.pose.position.x = -0.055;
            bbox_marker.pose.position.y = 0.09;            
            bbox_marker.pose.position.z = -left_shoulder_z_b - left_shoulder_z_a * i;
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 8 && i < 16)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015;
            bbox_marker.pose.position.y = 0.09;            
            bbox_marker.pose.position.z = -left_shoulder_z_b - left_shoulder_z_a * (i - 8);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 16 && i < 24)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * 2;
            bbox_marker.pose.position.y = 0.09;            
            bbox_marker.pose.position.z = -left_shoulder_z_b - left_shoulder_z_a * (i - 16);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 24 && i < 32)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * 3;
            bbox_marker.pose.position.y = 0.09;            
            bbox_marker.pose.position.z = -left_shoulder_z_b - left_shoulder_z_a * (i - 24);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 32 && i < 40)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * 4;
            bbox_marker.pose.position.y = 0.09;            
            bbox_marker.pose.position.z = -left_shoulder_z_b - left_shoulder_z_a * (i - 32);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 40 && i < 48)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * 5;
            bbox_marker.pose.position.y = 0.09;            
            bbox_marker.pose.position.z = -left_shoulder_z_b - left_shoulder_z_a * (i - 40);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 48 && i < 56)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * 6;
            bbox_marker.pose.position.y = 0.09;            
            bbox_marker.pose.position.z = -left_shoulder_z_b - left_shoulder_z_a * (i - 48);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }
        if(i >= 56 && i < 64)
        {
            bbox_marker.pose.position.x = -0.055 + 0.015 * 7;
            bbox_marker.pose.position.y = 0.09;            
            bbox_marker.pose.position.z = -left_shoulder_z_b - left_shoulder_z_a * (i - 56);
            roll = 1.570796327;
            pitch = 0.0;
            yaw = 0.0;
            q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
            bbox_marker.pose.orientation.x = q.x;
            bbox_marker.pose.orientation.y = q.y;
            bbox_marker.pose.orientation.z = q.z;
            bbox_marker.pose.orientation.w = q.w;
        }

        bbox_marker.lifetime = ros::Duration();
        marker_array.markers.push_back(bbox_marker);
    }

    while (marker_pub.getNumSubscribers() < 1)
    {
        if (!ros::ok())
        {
            return 0;
        }
        ROS_WARN_ONCE("Please create a subscriber to the marker");
        sleep(1);
    }
        marker_pub.publish(marker_array);

    //ros::spin();
    
    ros::spinOnce(); 
    
    loop_rate.sleep(); 
    //ros::spin();
    }
}
